#ifndef __BSP_CAN_MOTOR_H
#define __BSP_CAN_MOTOR_H


#include <stdint.h>
#include <stdbool.h>

// ==================== 麦轮电机ID配置宏 ====================
#define MOTOR_A_CAN_ID     0x201  // A电机CAN ID
#define MOTOR_B_CAN_ID     0x202  // B电机CAN ID  
#define MOTOR_C_CAN_ID     0x203  // C电机CAN ID
#define MOTOR_D_CAN_ID     0x204  // D电机CAN ID

#define MOTOR_CTRL_ALL_ID  0x200  // 所有电机控制ID
#define MOTOR_FB_MIN_ID    0x201
#define MOTOR_FB_MAX_ID    0x20F
// =======================================================

// 麦轮电机定义
typedef enum {
    MOTOR_A = 0,
    MOTOR_B = 1,  
    MOTOR_C = 2,
    MOTOR_D = 3,
    MOTOR_COUNT = 4
} motor_id_t;

// 电机反馈数据结构
typedef struct {
    // 原始反馈数据
    int16_t angle_raw;       // 原始角度
    int16_t velocity_raw;    // 原始速度
    int16_t current_raw;     // 原始电流
    
    // 处理后的数据
    float angle_deg;         // 角度(度)
    float velocity_rpm;      // 速度(RPM)
    float current_a;         // 电流(A)
    
    // 累计数据（用于PID）
    int32_t total_angle;     // 累计角度
    float total_angle_deg;   // 累计角度(度)
    
    // 状态信息
    uint32_t last_update;    // 最后更新时间
    bool connected;          // 是否连接
    uint32_t msg_count;      // 消息计数
} motor_feedback_t;

// 电机配置参数
typedef struct {
    float reduction_ratio;   // 减速比
    float max_current;       // 最大电流(A)
    float max_speed;         // 最大速度(RPM)
    int16_t current_to_raw;  // 电流到原始值转换系数
} motor_config_t;

// 电机控制函数
uint8_t bsp_can_motor_init(void);

// 电机控制接口
uint8_t bsp_can_send_motor_current(motor_id_t motor, int16_t current);
uint8_t bsp_can_send_all_motors_current(int16_t current_a, int16_t current_b, int16_t current_c, int16_t current_d);
uint8_t bsp_can_send_mecanum_current(int16_t vx, int16_t vy, int16_t vz);
uint8_t bsp_can_send_mecanum_velocity(float vx, float vy, float vz);

// 状态获取接口
motor_feedback_t* bsp_can_get_motor_feedback_ptr(motor_id_t motor);
motor_feedback_t bsp_can_get_motor_feedback(motor_id_t motor);
bool bsp_can_is_motor_connected(motor_id_t motor);
uint8_t bsp_can_get_connected_motors(void);

// 数据转换接口
float bsp_can_raw_angle_to_deg(int16_t raw_angle);
float bsp_can_raw_velocity_to_rpm(int16_t raw_velocity);
float bsp_can_raw_current_to_amp(int16_t raw_current);
int16_t bsp_can_amp_to_raw_current(float current_amp);
int16_t bsp_can_rpm_to_raw_velocity(float rpm);

// 电机配置
void bsp_can_set_motor_config(motor_id_t motor, motor_config_t config);
motor_config_t bsp_can_get_motor_config(motor_id_t motor);

// 电机数据处理
void bsp_can_motor_process(void);

// 电机探测
uint8_t bsp_can_detect_motors(uint16_t *motor_ids, uint8_t max_motors, uint32_t timeout_ms);

#endif